/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class BowelBot extends IterativeRobot {
    
    Joystick   DLeftJoy   = new Joystick(1);
    Joystick   DRightJoy  = new Joystick(2);
     
    CANJaguar  DFrontLeftJag;
    CANJaguar  DBackLeftJag;
    CANJaguar  DFrontRightJag;
    CANJaguar  DBackRightJag;
   
    RobotDrive Wheels;
    
    final int SpinnerChannel   = 0;
    final int ConveyorChannel  = 0;
            
    Victor Spinner    = new Victor(SpinnerChannel);
    Victor Conveyor   = new Victor(ConveyorChannel);
    Relay BallStop    = new Relay(0);
   
    Timer FeedTimer = new Timer(); 
   
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
public void robotInit() {
    
        try {
            
            DFrontLeftJag    = new CANJaguar(4);
            DBackLeftJag     = new CANJaguar(6);
            DFrontRightJag   = new CANJaguar(3);
            DBackRightJag    = new CANJaguar(7);
            
        } catch (CANTimeoutException ex) {}
        
        Wheels = new RobotDrive(DFrontLeftJag, DBackLeftJag, DFrontRightJag, DBackRightJag);
        
        Wheels.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        Wheels.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
        Wheels.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        Wheels.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);
        
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopContinuous() {
        
        if (DRightJoy.getRawButton(3)) {   
            Spinner.set(0.3);
            Conveyor.set(0.5);
        }
        if (DRightJoy.getRawButton(4)) {
            FeedTimer.reset();
            FeedTimer.start();
        
            while (FeedTimer.get() < 3) {
                BallStop.setDirection(Relay.Direction.kForward);
                BallStop.set(Relay.Value.kOn);
            }
            
            BallStop.set(Relay.Value.kOff);
        }
            
        Spinner.set(0);
        Conveyor.set(0);
        BallStop.set(Relay.Value.kOff);
        
        Wheels.tankDrive(DLeftJoy, DRightJoy);
        
    }
    
}
